#include <motorcontrol.hpp>

namespace wmj
{
    void ClearControl(wmj::Buffer &data_to_send, int &DataLength)
    {
        std::cout << "\nClearControl,stop move\n";
        uint8_t data[] = {0x01, 0xFF, 0xCA, 0x6B};
        for (auto c : data)
            data_to_send.push_back(c);
        DataLength = 6;
    }
    void StopControl(wmj::Buffer &data_to_send, int mode, int &DataLength)
    {
        std::cout << "\nstop move\n";
        if (mode == 0)
        {
            uint8_t data[] = {0x01, 0xFF, 0x10, 0x00, 0x00, 0x6B};
            for (auto c : data)
                data_to_send.push_back(c);
            DataLength = 6;
        }
        if (mode == 1)
        {
            uint8_t data[] = {0x01, 0xFF, 0x10, 0x00, 0xFF, 0x6B};
            for (auto c : data)
                data_to_send.push_back(c);
            DataLength = 6;
        }
    }
    void EnableControl(wmj::Buffer &data_to_send, int mode, int &DataLength)
    {
        std::cout << "\nEnable move\n";
        uint8_t data[] = {0x01, 0x0E, 0x52, 0x6B};
        for (auto c : data)
            data_to_send.push_back(c);
        DataLength = 6;
    }
    void VelocityControl(int dir, int speed, int Acceleratedspeed, wmj::Buffer &data_to_send, int &DataLength)
    {
        std::cout << "\nVelocityControl";
        if (dir == 1)
        {
            std::cout << "\t正方向";
        }
        else
        {
            std::cout << "\t反方向";
        }
        std::cout << "\t速度:" << speed << "\t加速度:" << Acceleratedspeed << std::endl;
        if (speed > 1279)
            speed = 1279;
        if (Acceleratedspeed > 256)
            Acceleratedspeed = 256;
        int16_t temp = ((uint16_t)dir << 12) | ((uint16_t)speed);
        uint8_t velocity_low = (uint8_t)((uint16_t)temp >> 8);
        uint8_t velocity_high = (uint8_t)((uint16_t)temp);
        uint8_t data[] = {0x01, 0xF6, velocity_low, velocity_high, 0x00, 0x6B};
        for (auto c : data)
            data_to_send.push_back(c);
        DataLength = 8;
    }
    void RotateEnable(wmj::Buffer &data_to_send, int &DataLength)
    {
        std::cout << "\nRotateEnable\n";
        uint8_t data[] = {0x01, 0x0E, 0x52, 0x6B};
        for (auto c : data)
            data_to_send.push_back(c);
        DataLength = 6;
    }
    void askForinfo(wmj::Buffer &data_to_send, int &DataLength)
    {
        std::cout << "\nask for info\n";
        uint8_t data[] = {0x01, 0x36, 0x6B};
        for (auto c : data)
            data_to_send.push_back(c);
        DataLength = 6;
    }
    int checkLength(wmj::Buffer data)
    {
        for (int i = 0; i < data.size(); i++)
        {
            if (data[i] == 0x6B)
            {
                // std::cout << i << std::endl;
                return i + 1;
            }
        }
    }
    void DrawPosition()
    {

    }
}